/*
 **********************************************************************
 * File name: quaternion.h
 *
 * COPYRIGHT (C) 2018-2022
 *
 * The license and distribution terms for this file may be found in 
 * the LICENSE file.
 *
 * Author: ChenLong (Add new editor)
 * Email: worldlong@foxmail.com
 **********************************************************************
*/
#pragma once
/*
*/
#include <cmath>
#include <cstring>
#include <cstdint>
#include "matrix.h"
#include "vector.h"
#include "euler.h"
#include "dcm.h"
/*
*/
namespace matrix
{
/*
*/
template<typename Type>
class Euler_T;

template<typename Type>
class DCM_T;

template<typename Type>
class Vector3;
/*---------------------------------------
                   Quternion
       Rotate from global to local
 *----------------------------------------*/
template<typename Type>
class Quaternion_T : public Vector<Type, 4>
{
public:
  /*
  */
  Type &q0 = this->data[0][0];
  Type &q1 = this->data[0][1];
  Type &q2 = this->data[0][2];
  Type &q3 = this->data[0][3];
  /*
  */
  Quaternion_T() {}
    
  Quaternion_T(Type qq0, Type qq1, Type qq2, Type qq3)
  {
    q0 = qq0;
    q1 = qq1;
    q2 = qq2;
    q3 = qq3;
  }
  
  Quaternion_T(Type qq0, const Vector<Type, 3> &vb)
  {
    q0 = qq0;
    q1 = vb.v[0];
    q2 = vb.v[1];
    q3 = vb.v[2];
  }
  
  Quaternion_T(const Vector<Type, 4> &vb) :
    Vector<Type, 4>(vb)
  {}
    
  Quaternion_T(const Quaternion_T<Type> &q)
  {
    q0 = q.q0;
    q1 = q.q1;
    q2 = q.q2;
    q3 = q.q3;
  }
  
  Quaternion_T(const Euler_T<Type> &euler, uint32_t type)
  {
    Type sr = sinf(euler.roll*0.5f);
    Type cr = cosf(euler.roll*0.5f);
    Type sp = sinf(euler.pitch*0.5f);
    Type cp = cosf(euler.pitch*0.5f);
    Type sy = sinf(euler.yaw*0.5f);
    Type cy = cosf(euler.yaw*0.5f);
    
    if(type == 321) //means rotating order is Z->Y->X
    {
      q0 = cr*cp*cy + sr*sp*sy;
      q1 = sr*cp*cy - cr*sp*sy;
      q2 = cr*sp*cy + sr*cp*sy;
      q3 = cr*cp*sy - sr*sp*cy;
    }
    else if(type == 312) //means rotating order is Z->X->Y
    {
      q0 = cp*cr*cy - sp*sr*sy;
      q1 = cp*sr*cy - sp*cr*sy;
      q2 = cp*sr*sy + sp*cr*cy;
      q3 = cp*cr*sy + sp*sr*cy;
    }
  }
  
  Quaternion_T(const DCM_T<Type> &dcm)
  {
    Type trace = dcm.trace();
    if(trace > 0) {
      q0 = sqrtf(1 + trace) / 2.0f;
      trace = 4 * q0;
      q1 = (dcm.data[1][2] - dcm.data[2][1]) / trace;
      q2 = (dcm.data[2][0] - dcm.data[0][2]) / trace;
      q3 = (dcm.data[0][1] - dcm.data[1][0]) / trace;
    } else if(dcm.data[2][2] > dcm.data[1][1] && dcm.data[2][2] > dcm.data[0][0]) {
      q3 = sqrtf(1 + 2*dcm.data[2][2] - trace) / 2.0f;
      trace = 4 * q3;
      q1 = (dcm.data[0][2] + dcm.data[2][0]) / trace;
      q2 = (dcm.data[1][2] + dcm.data[2][1]) / trace;
      q0 = (dcm.data[0][1] - dcm.data[1][0]) / trace;
    } else if(dcm.data[1][1] > dcm.data[0][0]) {
      q2 = sqrtf(1 + 2*dcm.data[1][1] - trace) / 2.0f;
      trace = 4 * q2;
      q1 = (dcm.data[0][1] + dcm.data[1][0]) / trace;
      q3 = (dcm.data[1][2] + dcm.data[2][1]) / trace;
      q0 = (dcm.data[2][0] - dcm.data[0][2]) / trace;
    } else {
      q1 = sqrtf(1 + 2*dcm.data[0][0] - trace) / 2.0f;
      trace = 4 * q1;
      q2 = (dcm.data[0][1] + dcm.data[1][0]) / trace;
      q3 = (dcm.data[0][2] + dcm.data[2][0]) / trace;
      q0 = (dcm.data[1][2] - dcm.data[2][1]) / trace;
    }
  }
  
  /*------------------------------------------*
   * Operator overload                        *
   *------------------------------------------*/
  /*
   * Q1 = Q2
   */
  Quaternion_T<Type> &operator=(const Quaternion_T<Type> &qb)
  {
    if (this != &qb) {
      memcpy(this->data, qb.data, sizeof(this->data));
    }
    return (*this);
  }
  
  /*
   * Q1 = V4
   */
  Quaternion_T<Type> &operator=(const Vector<Type, 4> &qb)
  {
    if (this != &qb) {
      memcpy(this->data, qb.data, sizeof(this->data));
    }
    return (*this);
  }
  
  /*
   * Q1 * Q2
   */
  Quaternion_T<Type> operator*(const Quaternion_T<Type> &qb) const
  {
    Quaternion_T<Type> vr;
   
    vr.q1 = q1*qb.q0 + q0*qb.q1 + q3*qb.q2 - q2*qb.q3;
    vr.q2 = q2*qb.q0 - q3*qb.q1 + q0*qb.q2 + q1*qb.q3;
    vr.q3 = q3*qb.q0 + q2*qb.q1 - q1*qb.q2 + q0*qb.q3;
    vr.q0 = q0*qb.q0 - q1*qb.q1 - q2*qb.q2 - q3*qb.q3;
    
    return vr;
  }

  /*
   * Q1 *= Q2
   */
  Quaternion_T<Type> &operator*=(const Quaternion_T<Type> &qb)
  {
    return *this * qb;
  }
  
  /*
   * Q1 * scalar
   */
  Quaternion_T<Type> operator*(Type scalar) const
  {
    Quaternion_T<Type> vr;
    vr.q0 = q0 * scalar;
    vr.q1 = q1 * scalar;
    vr.q2 = q2 * scalar;
    vr.q3 = q3 * scalar;
    
    return vr;
  }

  /*------------------------------------------*
   * Functions                                *
   *------------------------------------------*/
  inline Quaternion_T<Type> conjugate() const
  {
    Quaternion_T<Type> qr(q0, -q1, -q2, -q3);
    return qr;
  }
  
//  Quaternion_T<Type> derivative(const Vector3<Type> &vb) const
//  {     
//    Quaternion_T<Type> qr;
//    
//    qr.q0 = (-vb.x*q1 - vb.y*q2 - vb.z*q3) / 2;
//    qr.q1 = ( vb.x*q0 - vb.y*q3 + vb.z*q2) / 2;
//    qr.q2 = ( vb.x*q3 + vb.y*q0 - vb.z*q1) / 2;
//    qr.q3 = (-vb.x*q2 + vb.y*q1 + vb.z*q0) / 2;
//    
//    return qr;
//  }
//  
//  inline Quaternion_T<Type> dq(const Vector3<Type> &vb, Type dt) const
//  {
//    return this->derivative(vb) * dt;
//  }

  Vector3<Type> rotate(const Vector3<Type> &vb) const
  {
    Quaternion_T<Type> vq(0, vb.x, vb.y, vb.z);
    
    vq = *this * vq * this->conjugate();
    Vector3<Type> vr(vq.q1, vq.q2, vq.q3);
    
    return vr;
  }
  
  inline Vector3<Type> rotate_to_local(const Vector3<Type> &vb) const
  {
    return rotate(vb);
  }
  
  inline Vector3<Type> rotate_to_global(const Vector3<Type> &vb) const
  {
    Quaternion_T<Type> q = this->conjugate();
    return q.rotate(vb);
  }
  
};

typedef Quaternion_T<float> Quaternion;    //default Quaternion is float type

}





